#include "interpolator.h"
#include "external_libs/cx-lib/cx_typedef.h"
#include "sys.h"
#define IS_EUQAL(a,b) (a==b)
#define IS_GREATER(a,b) (a>b)

#define UIDTYPE unsigned int

void DDA_x_send_pulse(int dir);
void DDA_y_send_pulse(int dir);
void DDA_z_send_pulse(int dir);

#define DDA_INTERPOLATE_FINISHED_MASK (1<<0)
#define DDA_INTERPOLATE_MODE_LINE (1<<1)
#define DDA_INTERPOLATE_MODE_CCW_ARC (1<<2)
#define DDA_INTERPOLATE_MODE_CW_ARC (1<<3)
#define DDA_INTERPOLATE_ARC_X_FINISHED_MASK (1<<6)
#define DDA_INTERPOLATE_ARC_Y_FINISHED_MASK (1<<7)
#define DDA_INTERPOLATE_ARC_NOT_END_MASK (1<<8)

#define QUADRANT_I 0
#define QUADRANT_II 1
#define QUADRANT_III 2
#define QUADRANT_IV 3
#define QUADRANT_PX 4
#define QUADRANT_NX 5
#define QUADRANT_PY 6
#define QUADRANT_NY 7
#define QUADRANT_O  8


typedef void *interpolator_handler_args_t;
typedef void (*interpolator_handler_t)(interpolator_handler_args_t args);

static void null_interpolator_handler(void *args)
{

}

interpolator_handler_t cb_interpolator_handler = null_interpolator_handler;
interpolator_handler_args_t cb_interpolator_handler_args = CX_NULL;

void interpolator_Handler(void)
{
    cb_interpolator_handler(cb_interpolator_handler_args);
}

extern void extern_INTERPOLATOR_CANCLE(void)
{
    cb_interpolator_handler = null_interpolator_handler;
    cb_interpolator_handler_args = CX_NULL;
}

#define INTERPOLATOR_CANCLE() cb_interpolator_handler = null_interpolator_handler
#define INTERPOLATOR_REGISTER(handler,args) \
cb_interpolator_handler = (interpolator_handler_t)handler;\
cb_interpolator_handler_args = (interpolator_handler_args_t)args


void DDA_Line_Handler(DDA_Interpolator_pt dda)
{
    if(!(dda->flags & DDA_INTERPOLATE_FINISHED_MASK))
    {
        if((dda->flags & DDA_INTERPOLATE_MODE_LINE) != 0)
        {
            
        }
        if(dda->times < dda->m)
        {
            dda->i_x+=dda->x;
            dda->i_y+=dda->y;
            dda->i_z+=dda->z;
            if(dda->i_x >= dda->m)
            {
                dda->i_x-=dda->m;
                // DDA_x_send_pulse(dda->d_x);
                if(dda->d_x == 0)
                {
                    CNC_Move(1,0,0);
                }
                else
                {
                    CNC_Move(-1,0,0);
                }
                
            }
            if(dda->i_y >= dda->m)
            {
                dda->i_y-=dda->m;
                // DDA_y_send_pulse(dda->d_y);
                if(dda->d_y == 0)
                {
                    CNC_Move(0,1,0);
                }
                else
                {
                    CNC_Move(0,-1,0);
                }
            }
            if(dda->i_z >= dda->m)
            {
                dda->i_z-=dda->m;
                // DDA_z_send_pulse(dda->d_z);
                if(dda->d_z == 0)
                {
                    CNC_Move(0,0,1);
                }
                else
                {
                    CNC_Move(0,0,-1);
                }
            }
            dda->times++;
        }
        else
        {
            dda->flags |= DDA_INTERPOLATE_FINISHED_MASK;
        }
    }
}

IDTYPE DDA_get_max(IDTYPE x, IDTYPE y, IDTYPE z)
{
    IDTYPE ret;
    if(x>y)
    {
        if(x>z) ret = x;else ret = z;
    }
    else
    {
        if(y>z) ret = y;else ret = z;
    }
    return ret;
}
interpolator_err_t DDA_line(DDA_Interpolator_pt dda,IDTYPE x, IDTYPE y, IDTYPE z)
{

    IDTYPE m=1;
    IDTYPE main_dir;
    int dir_x=0,dir_y=0,dir_z=0;
    IDTYPE sigma_x=0,sigma_y=0,sigma_z=0;
    IDTYPE step_x=0,step_y=0,step_z=0;

    INTERPOLATOR_CANCLE();
    
    if(IS_GREATER(0,x))
    {
        x=0-x;
        dir_x = 1;
    }
    if(IS_GREATER(0,y))
    {
        y=0-y;
        dir_y = 1;
    }
    if(IS_GREATER(0,z))
    {
        z=0-z;
        dir_z = 1;
    }
    dda->d_x = dir_x;
    dda->d_y = dir_y;
    dda->d_z = dir_z;
    dda->x = x;
    dda->y = y;
    dda->z = z;
    dda->i_x=0;
    dda->i_y=0;
    dda->i_z=0;
    main_dir = DDA_get_max(x,y,z);
    for(;m<main_dir;m=m<<1);
    dda->m = m;
    dda->times = 0;
    dda->flags &= ~DDA_INTERPOLATE_FINISHED_MASK;
    dda->flags |= DDA_INTERPOLATE_MODE_LINE;
    INTERPOLATOR_REGISTER(DDA_Line_Handler,dda);
    return INTERPOLATOR_ERR_OK;
} 

interpolator_err_t point_init(ipoint_pt point, IDTYPE x, IDTYPE y, IDTYPE z)
{
    point->x = x;
    point->y = y;
    point->z = z;
    if(x>0)
    {
        if(y>0) point->quadrant = QUADRANT_I;
        else if(y<0) point->quadrant = QUADRANT_IV;
        else    point->quadrant = QUADRANT_PX;
    }
    else if(x<0)
    {
        if(y>0) point->quadrant = QUADRANT_II;
        else if(y<0) point->quadrant = QUADRANT_III;
        else point->quadrant = QUADRANT_NX;
    }
    else
    {
        if(y>0) point->quadrant = QUADRANT_PY;
        else if(y<0) point->quadrant = QUADRANT_NY;
        else point->quadrant = QUADRANT_O;
    }
    return INTERPOLATOR_ERR_OK;
}

int point_getDeltaQuadrant(ipoint_pt start, ipoint_pt end, ARC_DIR_enum_t dir)
{
    int ret = 0;
    if(start->quadrant == QUADRANT_O || end->quadrant == QUADRANT_O) return -1;
    if(dir == ARC_DIR_CW)
    {   
        ret = start->quadrant - end->quadrant;
        switch (start->quadrant)
        {
            case QUADRANT_PX:start->quadrant = QUADRANT_I;break;
            case QUADRANT_PY:start->quadrant = QUADRANT_II;break;
            case QUADRANT_NX:start->quadrant = QUADRANT_III;break;
            case QUADRANT_NY:start->quadrant = QUADRANT_IV;break;
            default:
                break;
        }
        switch (end->quadrant)
        {
            case QUADRANT_PX:end->quadrant = QUADRANT_I;break;
            case QUADRANT_PY:end->quadrant = QUADRANT_II;break;
            case QUADRANT_NX:end->quadrant = QUADRANT_III;break;
            case QUADRANT_NY:end->quadrant = QUADRANT_IV;break;
            default:
                break;
        }
    }
    else if(dir == ARC_DIR_CCW)
    {
        switch (start->quadrant)
        {
            case QUADRANT_PX:start->quadrant = QUADRANT_IV;break;
            case QUADRANT_PY:start->quadrant = QUADRANT_I;break;
            case QUADRANT_NX:start->quadrant = QUADRANT_II;break;
            case QUADRANT_NY:start->quadrant = QUADRANT_III;break;
            default:
                break;
        }
        switch (end->quadrant)
        {
            case QUADRANT_PX:end->quadrant = QUADRANT_IV;break;
            case QUADRANT_PY:end->quadrant = QUADRANT_I;break;
            case QUADRANT_NX:end->quadrant = QUADRANT_II;break;
            case QUADRANT_NY:end->quadrant = QUADRANT_III;break;
            default:
                break;
        }
        ret = end->quadrant - start->quadrant;
    }
    if(ret < 0)
    {
        ret += 4;
    }
    else if(ret == 0)
    {
        if(dir == ARC_DIR_CCW)
        {
            switch (start->quadrant)
            {
                case QUADRANT_I:ret = (end->y > start->y)?0:4;break;
                case QUADRANT_II:ret = (end->y < start->y)?0:4;break;
                case QUADRANT_III:ret = (end->y < start->y)?0:4;break;
                case QUADRANT_IV:ret = (end->y > start->y)?0:4;break;
                default:
                    break;
            }
        }
        else if(dir == ARC_DIR_CW)
        {
            switch (start->quadrant)
            {
                case QUADRANT_I:ret = (end->y < start->y)?0:4;break;
                case QUADRANT_II:ret = (end->y > start->y)?0:4;break;
                case QUADRANT_III:ret = (end->y > start->y)?0:4;break;
                case QUADRANT_IV:ret = (end->y < start->y)?0:4;break;
                default:
                    break;
            }
        }
        else {/*Nothing;*/}
            
    }
    return ret;
}

int sqrt_bv(int n)
{
    int sqrt = 0;
    int shift = 15;
    int sqrt2;
    while (shift >= 0)
    {
        sqrt2 = ((sqrt << 1) + (1 << shift)) << shift;
        if (sqrt2 <= n)
        {
            sqrt += (1 << shift);
            n -= sqrt2;
        }
        shift--;
    }
    return sqrt;
}

static void point_assign(ipoint_pt oped,ipoint_pt op)
{
    oped->x = op->x;
    oped->y = op->y;
    oped->z = op->z;
    oped->quadrant = op->quadrant;
}

// /**
//  * @brief get the next quadrant point array
//  * 
//  * @param array_quadrant_point 
//  * @param r 
//  * @param dir 
//  */
// static inline void point_setQuadrantPoint(ipoint_pt array_quadrant_point, IDTYPE r,ARC_DIR_enum_t dir)
// {
//     if(dir == ARC_DIR_CCW)
//     {
//         array_quadrant_point[0].x = 0;
//         array_quadrant_point[0].y = r;
//         array_quadrant_point[1].x = 0-r;
//         array_quadrant_point[1].y = 0;
//         array_quadrant_point[2].x = 0;
//         array_quadrant_point[2].y = 0-r;
//         array_quadrant_point[3].x = r;
//         array_quadrant_point[3].y = 0;
//     }
//     else if (dir == ARC_DIR_CW)
//     {
//         array_quadrant_point[0].x = r;
//         array_quadrant_point[0].y = 0;
//         array_quadrant_point[1].x = 0;
//         array_quadrant_point[1].y = r;
//         array_quadrant_point[2].x = 0-r;
//         array_quadrant_point[2].y = 0;
//         array_quadrant_point[3].x = 0;
//         array_quadrant_point[3].y = 0-r;
//     }
    
// }

/**
 * @brief get the next quadrant point array
 * 
 * @param array_quadrant_point 
 * @param r 
 * @param dir 
 */
interpolator_err_t point_getTransitionPoint(ipoint_pt pstart_point, IDTYPE r,ARC_DIR_enum_t dir,ipoint_pt presult)
{
    if(dir == ARC_DIR_CCW)
    {
        switch(pstart_point->quadrant)
        {
            case QUADRANT_I:presult->x=0;presult->y=r;presult->z=0;presult->quadrant=QUADRANT_PY;break;
            case QUADRANT_PX:presult->x=0;presult->y=r;presult->z=0;presult->quadrant=QUADRANT_PY;break;
            case QUADRANT_II:presult->x=0-r; presult->y=0; presult->z=0; presult->quadrant=QUADRANT_NX;break;
            case QUADRANT_PY:presult->x=0-r; presult->y=0; presult->z=0; presult->quadrant=QUADRANT_NX;break;
            case QUADRANT_III:presult->x=0; presult->y=0-r; presult->z=0; presult->quadrant=QUADRANT_NY;break;
            case QUADRANT_NX:presult->x=0; presult->y=0-r; presult->z=0; presult->quadrant=QUADRANT_NY;break;
            case QUADRANT_IV:presult->x=r; presult->y=0; presult->z=0; presult->quadrant=QUADRANT_PX;break;
            case QUADRANT_NY:presult->x=r; presult->y=0; presult->z=0; presult->quadrant=QUADRANT_PX;break;
            default:return INTERPOLATOR_ERR_ARC_INVALID_ORIGIN;break;
        }
    }
    else if (dir == ARC_DIR_CW)
    {
        switch(pstart_point->quadrant)
        {
            case QUADRANT_I:presult->x=r;presult->y=0;presult->z=0;presult->quadrant=QUADRANT_PX;break;
            case QUADRANT_PY:presult->x=r;presult->y=0;presult->z=0;presult->quadrant=QUADRANT_PX;break;
            case QUADRANT_II:presult->x=0; presult->y=r; presult->z=0; presult->quadrant=QUADRANT_PY;break;
            case QUADRANT_NX:presult->x=0; presult->y=r; presult->z=0; presult->quadrant=QUADRANT_PY;break;
            case QUADRANT_III:presult->x=0-r; presult->y=0; presult->z=0; presult->quadrant=QUADRANT_NX;break;
            case QUADRANT_NY:presult->x=0-r; presult->y=0; presult->z=0; presult->quadrant=QUADRANT_NX;break;
            case QUADRANT_IV:presult->x=0; presult->y=0-r; presult->z=0; presult->quadrant=QUADRANT_NY;break;
            case QUADRANT_PX:presult->x=0; presult->y=0-r; presult->z=0; presult->quadrant=QUADRANT_NY;break;
            default:return INTERPOLATOR_ERR_ARC_INVALID_ORIGIN;break;
        }
    }
    return INTERPOLATOR_ERR_OK;
    
}

cx_bool_t isPointsEquality(ipoint_pt ppoint1, ipoint_pt ppoint2)
{
    if((ppoint1->x == ppoint2->x) && (ppoint1->y == ppoint2->y) && (ppoint1->z == ppoint2->z))
    {
        return cx_true;
    }
    else
        return cx_false;
}

cx_bool_t Arc_isEndOnTransition(ipoint_pt pstart,ipoint_pt ptransition, ipoint_pt end)
{
    if((end->x==ptransition->x) || (end->y == ptransition->y))
    {
        if(end->x == ptransition->x)
        {
            if(((end->y >= 0) && (ptransition->y >= 0)) || ((end->y <=0) && (ptransition->y <= 0)))
            {
                return cx_true;
            }
            else
            {
                return cx_false;
            }
        }
        else if (end->y == ptransition->y)
        {
            if(((end->x >= 0) && (ptransition->x >= 0)) || ((end->x <= 0) && (ptransition->x <= 0)))
            {
                return cx_true;
            }
            else
            {
                return cx_false;
            }
        }
        else
        {
            return cx_true;
        }
    }
    else if(
    (end->x < ((ptransition->x > pstart->x)?ptransition->x:pstart->x)) && 
    (end->x > ((ptransition->x < pstart->x)?ptransition->x:pstart->x)) &&
    (end->y > ((ptransition->y < pstart->y)?ptransition->y:pstart->y)) && 
    (end->y < ((ptransition->y > pstart->y)?ptransition->y:pstart->y))
    )
    {
        return cx_true;
    }
    else
    {
        return cx_false;
    }
}

void DDA_Arc_Handler_CW(DDA_Interpolator_pt dda);
void DDA_Arc_Handler_CCW(DDA_Interpolator_pt dda);

interpolator_err_t DDA_Arc(DDA_Interpolator_pt dda,IDTYPE s_x, IDTYPE s_y, IDTYPE e_x, IDTYPE e_y, ARC_DIR_enum_t dir)
{
    ipoint_t transition_point;
    interpolator_err_t ret;
    float F = (system_feed_speed * system_feed_rate);
    INTERPOLATOR_CANCLE();
    point_init(&(dda->start),s_x,s_y,0);
    point_init(&(dda->end),e_x,e_y,0);
    dda->ARC_r = sqrt_bv(s_x*s_x + s_y * s_y);
    dda->ARC_dir = dir;
    ret = point_getTransitionPoint(&(dda->start),dda->ARC_r,dda->ARC_dir,&transition_point);
    if(Arc_isEndOnTransition(&(dda->start),&(transition_point),&(dda->end)) == cx_true)
    {
        point_assign(&(dda->temp_end), &(dda->end));
        dda->flags &= ~(DDA_INTERPOLATE_ARC_NOT_END_MASK);
    }
    else
    {
        point_assign(&(dda->temp_end), &(transition_point));
        dda->flags |= DDA_INTERPOLATE_ARC_NOT_END_MASK;
    }
    dda->d_x = (dda->start.x < dda->temp_end.x)?(dda->temp_end.x-dda->start.x):(dda->start.x-dda->temp_end.x);
    dda->d_y = (dda->start.y < dda->temp_end.y)?(dda->temp_end.y-dda->start.y):(dda->start.y-dda->temp_end.y);
    dda->i_x = 0;
    dda->i_y = 0;
    dda->overflow_value.m_x = (dda->ARC_r)*1000/system_interplate_period_ms/(F/60/X_MIN_MOTION_UNIT);
    dda->overflow_value.m_y = (dda->ARC_r)*1000/system_interplate_period_ms/(F/60/Y_MIN_MOTION_UNIT);
    dda->flags &= ~(DDA_INTERPOLATE_FINISHED_MASK | DDA_INTERPOLATE_ARC_X_FINISHED_MASK | DDA_INTERPOLATE_ARC_Y_FINISHED_MASK);
    dda->flags |= ((dda->ARC_dir == ARC_DIR_CCW)?DDA_INTERPOLATE_MODE_CCW_ARC:DDA_INTERPOLATE_MODE_CW_ARC);
    if(dda->ARC_dir == ARC_DIR_CCW)
    {
        INTERPOLATOR_REGISTER(DDA_Arc_Handler_CCW,dda);
    }
    else if(dda->ARC_dir == ARC_DIR_CW)
    {
        INTERPOLATOR_REGISTER(DDA_Arc_Handler_CW,dda);
    }
}

void DDA_Arc_Handler_CW(DDA_Interpolator_pt dda)
{
    static int temp_x0,temp_y0;
    if((dda->flags & DDA_INTERPOLATE_FINISHED_MASK) == 0)
    {
        temp_x0 = dda->start.x;
        temp_y0 = dda->start.y;
        if(dda->d_x > 0)
        {
            dda->i_x += temp_y0;
            if(dda->i_x < (0-dda->overflow_value.m_x))
            {
                dda->i_x += dda->overflow_value.m_x;
                dda->start.x--;
                // DDA_x_send_pulse(1);
                CNC_Move(-1,0,0);
                dda->d_x --;
            }
            else if(dda->i_x > dda->overflow_value.m_x)
            {
                dda->i_x -= dda->overflow_value.m_x;
                dda->start.x++;
                // DDA_x_send_pulse(0);
                CNC_Move(1,0,0);
                dda->d_x --;
            }
            else{}
        }
        if(dda->d_y > 0)
        {
            dda->i_y -= temp_x0;
            if(dda->i_y < (0-dda->overflow_value.m_y))
            {
                dda->i_y += dda->overflow_value.m_y;
                dda->start.y--;
                // DDA_y_send_pulse(1);
                CNC_Move(0,-1,0);
                dda->d_y --;
            }
            else if(dda->i_y > dda->overflow_value.m_y)
            {
                dda->i_y -= dda->overflow_value.m_y;
                dda->start.y++;
                // DDA_y_send_pulse(0);
                CNC_Move(0,1,0);
                dda->d_y--;
            }
            else{}
        }
    }
    if((dda->d_x == 0) && (dda->d_y == 0 ))
    {
        dda->flags |= DDA_INTERPOLATE_FINISHED_MASK;
        if(dda->flags & DDA_INTERPOLATE_ARC_NOT_END_MASK)
        {
            DDA_Arc(dda, dda->start.x, dda->start.y, dda->end.x, dda->end.y,dda->ARC_dir);
        }
    }
}

void DDA_Arc_Handler_CCW(DDA_Interpolator_pt dda)
{
    static int temp_x0,temp_y0;
    if((dda->flags & DDA_INTERPOLATE_FINISHED_MASK) == 0)
    {
        temp_x0 = dda->start.x;
        temp_y0 = dda->start.y;
        if(dda->d_x > 0)
        {
            dda->i_x -= temp_y0;
            if(dda->i_x < (0-dda->overflow_value.m_x))
            {
                dda->i_x += dda->overflow_value.m_x;
                dda->start.x--;
                // DDA_x_send_pulse(1);
                CNC_Move(-1,0,0);
                dda->d_x --;
            }
            else if(dda->i_x > dda->overflow_value.m_x)
            {
                dda->i_x -= dda->overflow_value.m_x;
                dda->start.x++;
                // DDA_x_send_pulse(0);
                CNC_Move(1,0,0);
                dda->d_x --;
            }
            else{}
        }
        if(dda->d_y > 0)
        {
            dda->i_y += temp_x0;
            if(dda->i_y < (0-dda->overflow_value.m_y))
            {
                dda->i_y += dda->overflow_value.m_y;
                dda->start.y--;
                // DDA_y_send_pulse(1);
                CNC_Move(0,-1,0);
                dda->d_y --;
            }
            else if(dda->i_y > dda->overflow_value.m_y)
            {
                dda->i_y -= dda->overflow_value.m_y;
                dda->start.y++;
                // DDA_y_send_pulse(0);
                CNC_Move(0,1,0);
                dda->d_y--;
            }
            else{}
        }
    }
    if((dda->d_x == 0) && (dda->d_y == 0 ))
    {
        dda->flags |= DDA_INTERPOLATE_FINISHED_MASK;
        if(dda->flags & DDA_INTERPOLATE_ARC_NOT_END_MASK)
        {
            DDA_Arc(dda, dda->start.x, dda->start.y, dda->end.x, dda->end.y,dda->ARC_dir);
        }
    }
}
// #define _USE_PC_DEBUG
#ifdef _USE_PC_DEBUG
#include "stdio.h"
#include "stdlib.h"
#include "pthread.h"
#include "sys.h"
IDTYPE coor[3] = {0,0,0};
void DDA_x_send_pulse(int dir)
{
    if(dir == 0)
        coor[0] ++;
    else
        coor[0] --;
}
void DDA_y_send_pulse(int dir)
{
    if(dir == 0)
        coor[1] ++;
    else
        coor[1] --;
}
void DDA_z_send_pulse(int dir)
{
    if(dir == 0)
        coor[2] ++;
    else
        coor[2] --;
}
int main(void)
{
    FILE *file;
    DDA_Interpolator_t dda_instance;
    DDA_Interpolator_pt pdda = &dda_instance;
    file = fopen("point.txt","w+");
    if(file == NULL)
    {
        printf("Error: Couldn't open\n");
        return 1;
    }
    fprintf(file,"x,y,z\n");
    // DDA_line(pdda,-200,100,0);
    CNC_SetFeedSpeed(600);
    // DDA_Arc(pdda, 250,0,-150,-200,ARC_DIR_CW);
    DDA_line(pdda, -300,200,0);
    for(int i=0; !(pdda->flags & DDA_INTERPOLATE_FINISHED_MASK); i++)
    {
        interpolator_Handler();
        fprintf(file, "%d,%d,%d\n", coor[0],coor[1],coor[2]);
    }
    fclose(file);
    system("python ../core/interpolator_visualizer.py");
    return 0;
}
#endif
